{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# Similarity2\n",
        "\n",
        "A `Similarity2` represents a similarity transformation in 2D space, which is a combination of a rotation, a translation, and a uniform scaling. It is an element of the special similarity group Sim(2), which is also a Lie group. Its 3-dimensional analog is `Similarity3`. It is included in the top-level `gtsam` package."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/geometry/doc/Similarity2.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "# Install GTSAM and Plotly from pip if running in Google Colab\n",
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop \n",
        "except ImportError:\n",
        "    pass # Not in Colab"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {},
      "outputs": [],
      "source": [
        "import gtsam\n",
        "from gtsam import Similarity2, Rot2, Point2, Pose2\n",
        "import numpy as np"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Initialization and Properties\n",
        "\n",
        "A `Similarity2` can be initialized in several ways:\n",
        "- With no arguments to create an identity transform `(R=I, t=0, s=1)`.\n",
        "- With a `Rot2` for rotation, a `Point2` for translation, and a `float` for scale."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Identity:\n",
            "\n",
            "\n",
            "R:\n",
            ": 0\n",
            "t: 0 0 s: 1\n",
            "\n",
            "S1:\n",
            "\n",
            "\n",
            "R:\n",
            ": 0.523599\n",
            "t: 10 20 s: 2\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Identity transform\n",
        "s_identity = gtsam.Similarity2()\n",
        "print(f\"Identity:\\n{s_identity}\")\n",
        "\n",
        "# Transform with 30-degree rotation, translation (10, 20), and scale 2\n",
        "R = Rot2.fromDegrees(30)\n",
        "t = Point2(10, 20)\n",
        "s = 2.0\n",
        "S1 = Similarity2(R, t, s)\n",
        "print(f\"S1:\\n{S1}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "The transform's properties can be accessed using `rotation()`, `translation()`, and `scale()`. The 3x3 matrix representation can be obtained with `matrix()`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Rotation:\n",
            "theta: 0.523599\n",
            "\n",
            "Translation: [10. 20.]\n",
            "Scale: 2.0\n",
            "Matrix:\n",
            "[[ 0.8660254 -0.5       10.       ]\n",
            " [ 0.5        0.8660254 20.       ]\n",
            " [ 0.         0.         0.5      ]]\n"
          ]
        }
      ],
      "source": [
        "print(f\"Rotation:\\n{S1.rotation()}\")\n",
        "print(f\"Translation: {S1.translation()}\")\n",
        "print(f\"Scale: {S1.scale()}\")\n",
        "print(f\"Matrix:\\n{S1.matrix()}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### Mathematical Representation\n",
        "\n",
        "GTSAM's `Similarity2` implementation follows a convention similar to that described in Ethan Eade's \"Lie Groups for 2D and 3D Transformations\". An element of Sim(2) is a tuple $(R, t, s)$ with $R \\in SO(2)$, $t \\in \\mathbb{R}^2$, and $s \\in \\mathbb{R}$.\n",
        "\n",
        "It can be represented by a $3 \\times 3$ matrix:\n",
        "$$\n",
        "T = \\begin{pmatrix} R & t \\\\ 0 & s^{-1} \\end{pmatrix} \\in \\text{Sim(2)}\n",
        "$$\n",
        "\n",
        "The composition of two transforms $T_1 = (R_1, t_1, s_1)$ and $T_2 = (R_2, t_2, s_2)$ is given by matrix multiplication:\n",
        "$$ \n",
        "T_1 \\cdot T_2 = \\begin{pmatrix} R_1 & t_1 \\\\ 0 & s_1^{-1} \\end{pmatrix} \\begin{pmatrix} R_2 & t_2 \\\\ 0 & s_2^{-1} \\end{pmatrix} = \\begin{pmatrix} R_1 R_2 & R_1 t_2 + s_2^{-1} t_1 \\\\ 0 & (s_1 s_2)^{-1} \\end{pmatrix}\n",
        "$$\n",
        "\n",
        "The inverse of a transform $T_1$ is:\n",
        "$$ \n",
        "T_1^{-1} = \\begin{pmatrix} R_1^T & -s_1 R_1^T t_1 \\\\ 0 & s_1 \\end{pmatrix}\n",
        "$$\n",
        "\n",
        "The action of a transform $T$ on a 2D point $p$ is defined as $p' = s(Rp + t)$. This can be derived from the matrix form by applying it to a homogeneous point $\\mathbf{x} = (p, 1)^T$ and re-normalizing:\n",
        "$$ \n",
        "T \\cdot \\mathbf{x} = \\begin{pmatrix} R & t \\\\ 0 & s^{-1} \\end{pmatrix} \\begin{pmatrix} p \\\\ 1 \\end{pmatrix} = \\begin{pmatrix} R p + t \\\\ s^{-1} \\end{pmatrix} \\thicksim \\begin{pmatrix} s(R p + t) \\\\ 1 \\end{pmatrix}\n",
        "$$"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Basic Operations\n",
        "\n",
        "`Similarity2` can transform `Point2` and `Pose2` objects."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "S1.transformFrom([1. 1.]) = [20.73205081 42.73205081]\n",
            "S1.transformFrom((5, 5, 0.785398)\n",
            ") = (23.6603, 53.6603, 1.309)\n",
            "\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Transform a Point2\n",
        "p1 = Point2(1, 1)\n",
        "p2 = S1.transformFrom(p1)\n",
        "print(f\"S1.transformFrom({p1}) = {p2}\")\n",
        "\n",
        "# Transform a Pose2\n",
        "pose1 = Pose2(Rot2.fromDegrees(45), Point2(5, 5))\n",
        "pose2 = S1.transformFrom(pose1)\n",
        "print(f\"S1.transformFrom({pose1}) = {pose2}\\n\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Lie Group Sim(2)\n",
        "\n",
        "`Similarity2` implements the Lie group operations `identity`, `inverse`, `compose`, and `between`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "S1 * S2 = \n",
            "\n",
            "\n",
            "R:\n",
            ": 1.5708\n",
            "t: 26.8301 38.1699 s: 1\n",
            "\n",
            "S1.inverse() = \n",
            "\n",
            "\n",
            "R:\n",
            ": -0.523599\n",
            "t: -37.3205  -24.641 s: 0.5\n",
            "\n",
            "S1.between(S2) = \n",
            "\n",
            "\n",
            "R:\n",
            ": 0.523599\n",
            "t: -72.8109 -56.1122 s: 0.25\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Create a second transform\n",
        "S2 = Similarity2(Rot2.fromDegrees(60), Point2(5, -5), 0.5)\n",
        "\n",
        "# Composition (group product)\n",
        "s_composed = S1 * S2\n",
        "print(f\"S1 * S2 = \\n{s_composed}\")\n",
        "\n",
        "# Inverse\n",
        "s_inv = S1.inverse()\n",
        "print(f\"S1.inverse() = \\n{s_inv}\")\n",
        "\n",
        "# Between (relative transform): S1.inverse() * S2 \n",
        "s_between = S1.between(S2)\n",
        "print(f\"S1.between(S2) = \\n{s_between}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### Lie Algebra\n",
        "\n",
        "The Lie algebra of Sim(2) is the tangent space at the identity. It is represented by a 4D vector `xi = [v_x, v_y, omega, lambda]`. \n",
        "- `(v_x, v_y)` is the translational velocity.\n",
        "- `omega` is the angular velocity.\n",
        "- `lambda` is the log of the rate of scale change.\n",
        "\n",
        "The `Expmap` and `Logmap` functions convert between the Lie algebra and the Lie group."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Expmap([0.1        0.2        0.78539816 0.40546511]) =\n",
            "\n",
            "\n",
            "R:\n",
            ": 0.785398\n",
            "t: 0.00791887   0.179002 s: 1.5\n",
            "\n",
            "Logmap(S_exp) = [0.1        0.2        0.78539816 0.40546511]\n"
          ]
        }
      ],
      "source": [
        "# Create a vector in the Lie algebra\n",
        "xi = np.array([0.1, 0.2, np.pi/4, np.log(1.5)])\n",
        "\n",
        "# Exponential map: from Lie algebra to group\n",
        "S_exp = Similarity2.Expmap(xi)\n",
        "print(f\"Expmap({xi}) =\\n{S_exp}\")\n",
        "\n",
        "# Logarithm map: from group to Lie algebra\n",
        "xi_log = Similarity2.Logmap(S_exp)\n",
        "print(f\"Logmap(S_exp) = {xi_log}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Manifold Operations\n",
        "\n",
        "`Similarity2` is also a manifold. The `retract` operation maps a tangent vector `v` from the tangent space at a `Similarity2` `p` to a new point on the manifold. The `localCoordinates` is the inverse operation.\n",
        "\n",
        "For Lie groups in GTSAM, `retract(v)` is equivalent to `p.compose(Expmap(v))`, and `localCoordinates(q)` is `Logmap(p.inverse().compose(q))`. "
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "localCoordinates(q) from p = [  0.48657861 -13.38262603   1.22173048   0.55961579]\n",
            "p.retract(v) =\n",
            "\n",
            "\n",
            "R:\n",
            ": 1.39626\n",
            "t:  8 -5 s: 2.1\n",
            "\n",
            "Original q =\n",
            "\n",
            "\n",
            "R:\n",
            ": 1.39626\n",
            "t:  8 -5 s: 2.1\n",
            "\n"
          ]
        }
      ],
      "source": [
        "p = Similarity2(Rot2.fromDegrees(10), Point2(1,2), 1.2)\n",
        "q = Similarity2(Rot2.fromDegrees(80), Point2(8,-5), 2.1)\n",
        "\n",
        "# Find the tangent vector to go from p to q\n",
        "v = p.localCoordinates(q)\n",
        "print(f\"localCoordinates(q) from p = {v}\")\n",
        "\n",
        "# Move from p along v to get back to q\n",
        "q_retracted = p.retract(v)\n",
        "print(f\"p.retract(v) =\\n{q_retracted}\")\n",
        "print(f\"Original q =\\n{q}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 2
}